#!/usr/bin/env python
# -*- coding: UTF-8 -*-
'''
@Project ：bignet_sumo_dev
@File ：main-dev-tongluoshan.py
@IDE ：PyCharm
@Author ：xinyingjie
@Date ：2025/3/24 9:40
'''

from __future__ import absolute_import
from __future__ import print_function

import json
import math
import time
from collections import defaultdict
from multiprocessing import Process, Queue

import sumolib
import traci.constants as tc
from kafka import KafkaProducer, KafkaConsumer
from loguru import logger

from config.config40 import GLOBAL_POLYGON_CENTER, MAX_SPEED, KAFKA_HOST, RADAR_DATA_LIST, KAFKA_SEND_TOPIC, \
    KAFKA_E1_TOPIC, KAFKA_E1_TOPIC_SIMU, SHOW_SIMU, USE_LIBSUMO, USE_GUI
from utils import sumo_utils
from utils.pursuit_utils import calculate_avg_speed, haversine, is_vehicle_passed_gantry, calculate_catch_down_time, \
    calculate_catch_up_time

if USE_LIBSUMO:
    import libsumo as traci
else:
    import traci

logger.add("debug.log", format="{time} {level} {message}", level="DEBUG")


def send_kafka_1(q, p_id):
    kafka_producer = KafkaProducer(bootstrap_servers=KAFKA_HOST, key_serializer=str.encode,
                                   value_serializer=lambda x: json.dumps(x).encode('utf-8'),
                                   compression_type='gzip')
    send_time = 0
    send_count = 0
    frame_num = 0
    while True:
        # logger.info(f"{q.qsize()}")
        while not q.empty():
            frame_num += 1
            t1 = time.time()
            data = q.get()
            # print("shuju",data)

            kafka_producer.send(KAFKA_SEND_TOPIC + str(p_id), value=data,
                                key=str(int(time.time() * 1000)))
            end_time = time.time()
            if end_time - send_time > 1:
                logger.info(f"进程{p_id} 发送帧率 {abs(send_count - frame_num)}  发送队列长度{q.qsize()}")
                send_time = end_time
                send_count = frame_num
        time.sleep(0.05)


def read_trafficE1_channel(target_channel, q_r, q_s, p_id):
    kafka_producer = KafkaProducer(bootstrap_servers=KAFKA_HOST, key_serializer=str.encode,
                                   value_serializer=lambda x: json.dumps(x).encode('utf-8'),
                                   compression_type='gzip')
    # 实时接收全域E1帧数据
    kafka_consumer_E1 = KafkaConsumer(
        KAFKA_E1_TOPIC,
        bootstrap_servers=KAFKA_HOST,
        auto_offset_reset='latest',
        enable_auto_commit=False
    )
    logger.info(f"全域E1{target_channel}监听已就绪")
    RADAR_sn_set = set(RADAR_DATA_LIST.keys())
    # 存储每一辆车的多帧信息
    per_car_info_dict = {}
    sn_car_info_dict = {}
    for sn in RADAR_sn_set:
        per_car_info_dict[sn] = {}
        sn_car_info_dict[sn] = {}
    simu_json_list_before = []
    while True:
        time.sleep(0.1)
        send_time = 0
        send_count = 0
        frame_num = 0
        for message in kafka_consumer_E1:
            if message:
                # try:
                frame_num += 1
                sn_set = set()
                one_list = []
                message_value = message.value.decode('utf-8')
                data = json.loads(message_value)
                # TODO 将数据缓存添加到仿真进程内
                q_r.put(data)
                simu_json_list = []
                while not q_s.empty():
                    simu_json_list = q_s.get()
                    # print("发送端", len(simu_json_list))
                if len(simu_json_list) > 0:
                    simu_json_list_before = simu_json_list
                if SHOW_SIMU:
                    data['e1FrameParticipant'] = one_list + simu_json_list_before if len(
                        simu_json_list_before) > 0 else one_list
                else:
                    data['e1FrameParticipant'] = one_list
                # logger.info(f"感知数量为{len(one_list)} 仿真数量为{len(self.simu_json_list_before)}")
                kafka_producer.send(KAFKA_E1_TOPIC_SIMU, value=data,
                                    key=str(int(time.time())))
                error_sn_set = RADAR_sn_set - sn_set
                # logger.error(f"故障雷达{error_sn_set}")
                for sn in RADAR_sn_set:
                    if sn in error_sn_set:
                        RADAR_DATA_LIST[sn]['normal'] = False
                    else:
                        RADAR_DATA_LIST[sn]['normal'] = True
                end_time = time.time()
                if end_time - send_time > 1:
                    logger.info(
                        f"进程{p_id} 发送帧率 {abs(send_count - frame_num)}  加车队列长度{q_r.qsize()} 发送队列长度{q_s.qsize()}")
                    send_time = end_time
                    send_count = frame_num = 0

                    # self.kafka_producer.send(KAFKA_E1_TOPIC_SIMU, value=data,
                    #                          key=str(int(time.time())))
                # except Exception as e:
                #     logger.error(str(e))


class SumoMain:
    def __init__(self, sumo_net_file, sumo_cfg_file, gui, process_id):
        """
        仿真初始化
        :param sumo_net_file:  仿真路网文件路径
        :param sumo_cfg_file:  仿真配置文件路径
        :param gui: 是否开启gui
        :param process_id: 进程id
        """
        # ===========================路网相关数据=====================================
        self.sumo_net_file = sumo_net_file
        self.sumo_cfg_file = sumo_cfg_file
        self.gui = gui
        # 仿真路网对象
        self.sumo_net_instance = None

        # ===========================进程相关数据=====================================
        self.process_id = process_id
        # 是否开启速度控制
        self.speed_control = False

        # ===========================雷达设备分布=====================================
        # 雷达编码
        self.RADAR_sn_set = set(RADAR_DATA_LIST.keys())
        self.plate_radar = []
        self.miss_plate_radar = []

        # =============================数据相关======================================
        # 存储门架此时未匹配断面的轨迹数据，如果转化为断面后，移除该数据
        self.sn_between_car_info_dict = defaultdict(lambda: defaultdict(dict))
        # 门架对应断面数据
        self.sn_gantry_info_dict = defaultdict(dict)
        # 存储卡口、雷达经纬度所在路网的路段和位置信息
        self.kakou_pos_dict = {}
        # 发车信息
        self.sn_send_car_info_dict = dict()
        # 车辆速度持续时间
        self.sn_send_car_match_info_dict = dict()
        # 存储每一辆车的多帧信息
        self.per_car_info_dict = {}
        self.sn_car_info_dict = dict()
        for sn in self.RADAR_sn_set:
            self.sn_car_info_dict[sn] = {}
            self.per_car_info_dict[sn] = {}
        self.simu_json_list = []
        self.simu_json_list_before = []
        self.sn_car_info_dict = {}
        self.add_car_station_info = {}
        # 正在加速列表
        self.speed_accelerate_list = []
        # 正在减速列表
        self.speed_decelerate_list = []
        # 缓存持续时长/原始速度
        self.sim_speed_cache_dict = dict()
        self.init_sumo()
        # self.executor = concurrent.futures.ThreadPoolExecutor(max_workers=20)
        self.global_vehicles = None
        self.batch_num = 0
        self.global_time = time.time()
        self.global_num = 0
        self.start_time = int(time.time())

    def init_sumo(self):
        """
        初始化sumo  加载
        :return:
        """
        # 加载路网
        self.sumo_net_instance = sumolib.net.readNet(self.sumo_net_file, withInternal=True, withLatestPrograms=True)
        # 启动仿真
        sumo_utils.start_sumo(self.sumo_cfg_file, False, gui=self.gui)
        # 设置所有车道的最大通行速度
        for id in traci.lane.getIDList():
            traci.lane.setMaxSpeed(id, MAX_SPEED)
        # 新增区域中心矩形，用于订阅100KM范围内的所有仿真车辆信息，并配置监听信息内容
        traci.polygon.add('GLOBAL_POLYGON_CENTER', GLOBAL_POLYGON_CENTER, (1, 0, 1))
        traci.polygon.subscribeContext('GLOBAL_POLYGON_CENTER',
                                       tc.CMD_GET_VEHICLE_VARIABLE, 1000000,
                                       [tc.VAR_SPEED, tc.VAR_TYPE, tc.VAR_POSITION, tc.VAR_ANGLE, ])

        # 将雷达、卡口相机的经纬度和终点位置转为路网上所在的路段id 位置距离 车道号，起始点和终点所在的路段id添加路由
        for sn, sn_info in RADAR_DATA_LIST.items():
            # 转换经纬度到路网坐标
            center_x, center_y = traci.simulation.convertGeo(sn_info['lon'], sn_info['lat'], fromGeo=True)
            EdgeID, Pos, LaneIndex = traci.simulation.convertRoad(sn_info['lon'], sn_info['lat'], True)
            next_edge_id, next_pos = sumo_utils.get_downstream_position_simplified(self.sumo_net_instance, EdgeID, Pos,
                                                                                   1)
            arrive_edge_id, arrive_pos = sumo_utils.get_downstream_position_simplified(self.sumo_net_instance, EdgeID,
                                                                                       Pos, 100)
            # arrive_edge_id, arrive_pos = sumo_utils.get_downstream_position_simplified(self.sumo_net_instance, EdgeID,
            #                                                                            Pos, 150)

            logger.info(
                f"门架{sn_info['sn']} 所在路段为{EdgeID} 位置为{Pos} 下行100米后的路段为{next_edge_id} 位置为{next_pos}")
            if sn_info['sn'] not in self.kakou_pos_dict.keys():
                traci.route.add(str(sn_info['sn']), [str(next_edge_id), str(-87)])
                self.kakou_pos_dict[str(sn_info['sn'])] = {"EdgeID": next_edge_id, "Pos": next_pos,
                                                           'LaneIndex': LaneIndex,
                                                           'routeId': str(str(sn_info['sn']))}
            # 创建圆形区域，表示相机或雷达的位置，用于在使用gui可视化时，展示设备具体位置
            if sn_info['normal']:
                # 创建圆形区域
                self.create_circle_polygon(sn, center_x, center_y, radius=50)
            else:
                self.create_circle_polygon(sn, center_x, center_y, radius=50, color=(255, 0, 0, 255))

    def create_circle_polygon(self, vehID, center_x, center_y, radius, color=(0, 255, 0, 255), filled=True):
        """
        在指定位置创建一个近似圆形的多边形。

        :param vehID: 车辆 ID 或其他唯一标识符，用于命名多边形
        :param center_x: 圆心的 x 坐标
        :param center_y: 圆心的 y 坐标
        :param radius: 圆的半径
        :param color: 多边形的颜色 (R,G,B,A)，默认绿色不透明
        :param filled: 是否填充多边形，默认 True
        """
        # 生成圆周上的点
        num_points = 50  # 点的数量越多，圆越光滑
        points = []
        for i in range(num_points):
            angle = 2 * math.pi * i / num_points
            x = center_x + radius * math.cos(angle)
            y = center_y + radius * math.sin(angle)
            points.append((x, y))

        # 添加多边形
        polygon_id = f"circle_{vehID}"
        traci.polygon.add(polygon_id, shape=points, color=color, fill=filled, layer=0)

    def add_gantry_info(self, car_info, source):
        car_id = car_info['sn_id']
        set_speed = car_info['avg_speed']
        # print('sn', car_info)
        traci.vehicle.add(
            vehID=str(car_id),
            routeID=self.kakou_pos_dict[car_info['sn']]['routeId'],
            departPos=self.kakou_pos_dict[str(car_info['sn'])]['Pos'],
            typeID=int(car_info['type']), depart='now',
            departSpeed=str(set_speed))
        traci.vehicle.setSpeed(str(car_id),
                               set_speed)
        self.sn_send_car_info_dict[str(car_id)] = car_info['plate']
        # print('sn', car_info['sn'])
        leida_info = RADAR_DATA_LIST[car_info['sn']]
        down_stream = leida_info['Downstream']
        if down_stream:
            sn = down_stream['sn']
        else:
            sn = None
        self.sn_send_car_match_info_dict[str(car_id)] = {
            'sn': sn,
            'time': int(time.time() * 1000),
            't_acc': 0,
            't_speed': set_speed,
            't_const': 0,
            'source': source,
            'flag': False
        }

    def main_loop(self):
        global re_queue_data

        # 在子进程中初始化多进程队列和资源
        queue_data = Queue(maxsize=2000)  # 多进程队列在此处创建
        # Process(target=send_kafka_1, args=(queue_data, self.process_id,)).start()
        # 在子进程中初始化多进程队列和资源
        re_queue_data = Queue(maxsize=2000)  # 多进程队列在此处创建
        Process(target=read_trafficE1_channel,
                args=(KAFKA_E1_TOPIC, re_queue_data, queue_data, self.process_id), daemon=True).start()
        batch_num = 0

        while True:
            # try:
            start_time = time.time()
            start_time_m = int(time.time() * 1000)

            # ================================收到门架上报数据，进行车辆处理==========================================

            global_vehicles = traci.polygon.getContextSubscriptionResults('GLOBAL_POLYGON_CENTER')
            self.global_vehicles = global_vehicles
            # todo 发车控制（目前直接写入回调中，后续可考虑提出）
            self.control_send_car()
            self.control_speed_car()
            # self.control_car(global_vehicles)
            # self.stop_remove_car(global_vehicles, queue_data)
            # self.executor.submit(self.stop_remove_car, global_vehicles.copy(), queue_data)
            self.batch_num = batch_num
            batch_num += 1
            traci.simulationStep()
            end_time = time.time()
            # print(end_time - self.global_time)
            if end_time - self.global_time > 1:
                logger.info(
                    f"进程{self.process_id} 仿真帧率 {abs(self.global_num - batch_num)} 仿真数量{len(global_vehicles)}")
                self.global_time = end_time
                self.global_num = batch_num
            if start_time and (end_time - start_time) < 0.1:
                time.sleep(0.1 - (end_time - start_time))
            else:
                if start_time:
                    pass
            # except Exception as e:
            #     logger.error(f"主循环异常{str(e)}")
            #     continue

    def control_send_car(self):
        start_time = int(time.time() * 1000)
        # 获取所有门架数据
        while not re_queue_data.empty():
            data = re_queue_data.get(block=False)
            sn_set = set()
            objs = data['obj']
            ts = data['ts']
            for obj in objs:
                sn = str(obj['sn'])
                if sn in ['']:
                    continue
                obj['sourceId'] = sn
                sn_set.add(sn)
                car_id = obj['id']
                obj['plate'] = obj['id']
                # 存储车辆经过门架的所有信息
                if car_id not in self.per_car_info_dict:
                    self.per_car_info_dict[sn][car_id] = {}
                    self.per_car_info_dict[sn][car_id][ts] = obj
                else:
                    self.per_car_info_dict[sn][car_id][ts] = obj
        print('门架数据获取总耗时：', int(time.time() * 1000) - start_time)
        # 判断门架数据是否已满足断面数据条件
        # 遍历所有门架，计算断面数据
        per_car_info_dict = self.per_car_info_dict.copy()
        for sn_id in per_car_info_dict.keys():
            sn_info = per_car_info_dict.get(sn_id)
            leida_info = RADAR_DATA_LIST[sn_id]
            for car_id in sn_info.keys():
                car_infos = sn_info[car_id]
                if sn_id in self.sn_gantry_info_dict.keys():
                    if car_id not in self.sn_gantry_info_dict[sn_id].keys():
                        result = calculate_avg_speed(car_infos, 10, leida_info['lon'], leida_info['lat'])
                        if result:
                            car_id, avg_speed, lon, lat, sn_r_id, plate, c_type = result
                            self.sn_gantry_info_dict[sn_id][car_id] = {
                                'id': car_id,
                                'avg_speed': avg_speed,
                                'lon': lon,
                                'lat': lat,
                                'sn_id': sn_r_id,
                                'plate': plate,
                                'sn': sn_id,
                                'type': c_type,
                                'flag': True
                            }
                else:
                    result = calculate_avg_speed(car_infos, 10, leida_info['lon'], leida_info['lat'])
                    if result:
                        car_id, avg_speed, lon, lat, sn_r_id, plate, c_type = result
                        self.sn_gantry_info_dict[sn_id][car_id] = {
                            'id': car_id,
                            'avg_speed': avg_speed,
                            'lon': lon,
                            'lat': lat,
                            'sn_id': sn_r_id,
                            'plate': plate,
                            'sn': sn_id,
                            'type': c_type,
                            'flag': True,
                            'time': int(time.time() * 1000)
                        }
        print('断面数据计算获取总耗时：', int(time.time() * 1000) - start_time)
        # 对断面数据进行发车
        # -携带身份信息    -未携带身份信息
        sn_gantry_info_dict = self.sn_gantry_info_dict.copy()
        for sn, car_infos in sn_gantry_info_dict.items():
            for car_id, car_info in car_infos.items():
                plate = car_info['plate']
                # 首先判断车牌是否存在
                if plate is not '':
                    if plate not in self.sn_send_car_info_dict.values():
                        # 判断车辆是否发车
                        if car_info['flag']:
                            self.add_gantry_info(car_info, 0)
                        car_info['flag'] = False
                        self.sn_gantry_info_dict[sn][car_id] = car_info
                else:
                    # todo 无车牌数据匹配
                    # 判断雷达数据是否有匹配数据
                    pass
        print('发车控制处理总耗时：', int(time.time() * 1000) - start_time)
        # 对于已经发车的车辆进行数据匹配
        # 遍历所有车辆,判断下一个门架中是否有匹配数据
        global_vehicles = self.global_vehicles.copy()
        sn_send_car_match_info_dict = self.sn_send_car_match_info_dict.copy()
        sn_gantry_info_dict = self.sn_gantry_info_dict.copy()
        for vehicle_id, veh_info in dict(global_vehicles).items():
            match_info = sn_send_car_match_info_dict.get(vehicle_id)
            down_sn = match_info['sn']
            plate = self.sn_send_car_info_dict.get(vehicle_id)
            if down_sn is None:
                continue
            # print(down_sn,  '门架信息：', sn_gantry_info_dict)
            if down_sn in sn_gantry_info_dict.keys():
                car_infos = sn_gantry_info_dict.get(down_sn)
                if plate in car_infos.keys():
                    leida_info = RADAR_DATA_LIST[down_sn]
                    down_stream = leida_info['Downstream']
                    if down_stream:
                        match_info['sn'] = down_stream['sn']
                    else:
                        match_info['sn'] = None
                    self.sn_send_car_match_info_dict[vehicle_id] = match_info
                    car_info = car_infos.get(plate)
                    pos = veh_info[tc.VAR_POSITION]
                    ang = veh_info[tc.VAR_ANGLE]
                    speed = veh_info[tc.VAR_SPEED]
                    longitude, latitude = traci.simulation.convertGeo(pos[0], pos[1], fromGeo=False)
                    pass_flag = is_vehicle_passed_gantry(longitude, latitude, ang, car_info['lon'],
                                                         car_info['lat'])
                    logger.info(f'{vehicle_id} 车辆经过门架： {down_sn}， 匹配车牌为：{plate}')
                    if pass_flag:
                        result = calculate_catch_down_time(speed, car_info['avg_speed'], 10, 20,
                                                           longitude,
                                                           latitude, car_info['lon'],
                                                           car_info['lat'])
                        t_acc, t_const = result
                        avg_speed = car_info['avg_speed']
                        _distance = haversine(car_info['lon'], car_info['lat'], longitude, latitude)
                        logger.info(
                            f'{vehicle_id} 车辆经过门架： {down_sn}， 距离门架距离为{_distance}米，当前速度为：{speed},'
                            f' 断面速度为  {avg_speed} 做减速，减速时间为：{t_acc}  持续时间为：{t_const}')
                        if t_const:
                            # 说明能追上
                            traci.vehicle.slowDown(vehicle_id, speed * 0.8, t_acc)
                            self.sim_speed_cache_dict[vehicle_id] = {
                                'sn': down_sn,
                                't_acc': t_acc,
                                't_const': t_const,
                                'time': int(time.time()),
                                'i_speed': car_info['avg_speed']
                            }
                    else:
                        result = calculate_catch_up_time(speed, car_info['avg_speed'], 5, 20,
                                                         longitude,
                                                         latitude, car_info['lon'],
                                                         car_info['lat'])
                        t_acc, t_const = result
                        avg_speed = car_info['avg_speed']
                        _distance = haversine(car_info['lon'], car_info['lat'], longitude, latitude)
                        logger.info(
                            f'{vehicle_id} 车辆未经过门架： {down_sn}， 距离门架距离为{_distance}米，当前速度为：{speed},'
                            f' 断面速度为  {avg_speed} 做加速，加速时间为：{t_acc}  持续时间为：{t_const}')
                        if t_const:
                            # 说明能追上
                            traci.vehicle.slowDown(vehicle_id, speed * 1.2, t_acc)
                            self.sim_speed_cache_dict[vehicle_id] = {
                                'sn': down_sn,
                                't_acc': t_acc * 1000,
                                't_const': t_const * 1000,
                                'time': int(time.time() * 1000),
                                'i_speed': car_info['avg_speed']
                            }
        print('速度控制计算总耗时：', int(time.time() * 1000) - start_time)

    def control_speed_car(self):
        # 速度控制
        # 获取全局车辆 id
        all_vehicle_keys = self.global_vehicles.copy()
        sim_speed_cache_dict = self.sim_speed_cache_dict.copy()
        for global_car_id, sim_speed_cache in sim_speed_cache_dict.items():
            t_acc = sim_speed_cache['t_acc']
            t_const = sim_speed_cache['t_const']
            s_time = sim_speed_cache['time']
            i_speed = sim_speed_cache['i_speed']
            sn = sim_speed_cache['sn']
            n_time = time.time() * 1000
            if n_time - s_time - t_acc - t_const > 0:
                if global_car_id in all_vehicle_keys:
                    # 速度变化后，移除车辆
                    traci.vehicle.setSpeed(global_car_id, i_speed)
                    logger.info(f'经过门架：{sn}, 车辆{global_car_id} 速度变化结束，恢复初始速度{i_speed}')
                    self.sim_speed_cache_dict.pop(global_car_id)


def run_sumo_simulation(sumo_net_file, sumo_cfg_file, gui, process_id):
    sumo_main = SumoMain(sumo_net_file, sumo_cfg_file, gui, process_id)
    sumo_main.main_loop()


if __name__ == '__main__':
    # 路网文件
    sumo_net_file = "./net/40KM.net.xml"
    # 配置文件
    sumo_cfg_file = "./net/40KM.sumocfg"
    # 是否启用gui可视化界面，调试阶段启用，线上部署关闭减少资源占用

    gui = USE_GUI
    for i in range(1):
        print("开启线程")
        tess_process = Process(target=run_sumo_simulation, args=(sumo_net_file, sumo_cfg_file, gui, i))
        tess_process.start()
